#pragma ident "$Id: XY. CAO 2017-08-15 $"

/**
* @file CorrectBiasWL.hpp
* Class to correct observables with satellite upds.
*/

//============================================================================
//
//  XY. CAO, Wuhan University. 2017
//  shjzhang, 2020/03/01, modify the format
//
//============================================================================


#include "CorrectBiasWL.hpp"
#include "constants.hpp"

#define debug 0

namespace gpstk
{

	// Index initially assigned to this class
	int CorrectBiasWL::classIndex = 4900000;


	// Returns an index identifying this object.
	int CorrectBiasWL::getIndex() const
	{ return index; }


	// Returns a string identifying this object.
	std::string CorrectBiasWL::getClassName() const
	{ return "CorrectBiasWL"; }


	/* Returns a satTypeValueMap object, adding the new data generated
	*  when calling this object.
	*
	* @param time      Epoch corresponding to the data.
	* @param gData     Data object holding the data.
	*/
	satTypeValueMap& CorrectBiasWL::Process( const CommonTime& time, 
                                             satTypeValueMap& gData )
		noexcept(false)
	{
		try
		{
			SatIDSet satRejectedSet;

            if(debug)
            cout << "time:" << time << endl;

			// Loop through all the satellites
			for (satTypeValueMap::iterator stv = gData.begin(); 
				stv != gData.end(); 
				++stv)
			{
                TypeIDSet typeSetRejected;

                typeValueMap wlBiasData;
				try
				{
					// Get satellite upds from 'RinexUPDStore'
					wlBiasData = (*pBiasStore).getValue( (*stv).first, time );

                    if(debug)
                    {
                        for(auto bd: wlBiasData)
                        {
                            cout << (*stv).first << " :"
                                 << bd.first << " :" 
                                 << bd.second << endl;;
                        }
                    }
				}
				catch(InvalidRequest& e)
				{
					// If some problem appears, then schedule this satellite
					// for removal
                    if(debug)
                    cerr << "WL upd can't be found for sat:"<< (*stv).first << endl;
					satRejectedSet.insert( (*stv).first );
					continue;    // Skip this SV if problems arise
				}

				// Warning:
				// you should check the unit of the UPDs in the input file.
				// if it is cycles, then convert it to meters firstly, 
				// or if it is meters, you can use it directly.

				SatID tempSat = (*stv).first;			
				if( tempSat.system == SatelliteSystem::GPS )
				{
                    try
                    {
                        // unit is cycle
                        double updBWL12 = wlBiasData.getValue(TypeID::BWL12); 

                        double MW12 = (*stv).second(TypeID::MW12);

                        if(debug)
                        cout << "sat:" << tempSat << "MW12:" << MW12;

				        // Now, correct the Melboune-Wubbena combination observables
				        // for every GNSS satellites.
				        (*stv).second[TypeID::MW12] = MW12 - WL12_WAVELENGTH_GPS*updBWL12;

                    }
                    catch(...)
                    {
                        // If some value is missing, then schedule this satellite
                        // for removal
                        satRejectedSet.insert( tempSat );
                        continue;
                    }

                    try
                    {
                        double updBWL15 = wlBiasData.getValue(TypeID::BWL15); 
                        double MW15 = (*stv).second(TypeID::MW15);
                        
				        // Now, correct the Melboune-Wubbena combination observables
				        // for every GNSS satellites.
				        (*stv).second[TypeID::MW15] = MW15 - WL15_WAVELENGTH_GPS*updBWL15;

                    }
                    catch(...)
                    {
                        // If some value is missing, then schedule this satellite
                        // for removal
                        typeSetRejected.insert(TypeID::MW15);
                        typeSetRejected.insert(TypeID::LC15);
                        typeSetRejected.insert(TypeID::L5);
                        continue;
                    }

				}
				else if( tempSat.system == SatelliteSystem::Galileo )
				{
				}
				else if( tempSat.system == SatelliteSystem::BDS)
				{
				}	
				else if( tempSat.system == SatelliteSystem::GLONASS)
				{

// please modify
/*
					int k(0);

					try
					{
						// Get frequency number 
						k = (*stv).second[TypeID::FreqNo];

					}
					catch(InvalidRequest& e)
					{
						// If some problem appears, then schedule this satellite
						// for removal. Thiis may be caused GLONASS not processed
						satRejectedSet.insert( (*stv).first );

						continue;    // Skip this SV if problems arise

					}

					// Frequency for this satellite
					double GLO_L1_FRE = L1_FREQ_GLO + L1_FREQ_STEP_GLO*k;
					double GLO_L2_FRE = L2_FREQ_GLO + L2_FREQ_STEP_GLO*k;
					double L1_WAVE_GLO = C_MPS/GLO_L1_FRE;
					double L2_WAVE_GLO = C_MPS/GLO_L2_FRE;
					double WL_WAVE_GLO = C_MPS/(GLO_L1_FRE - GLO_L2_FRE);
					double NL_WAVE_GLO = C_MPS/(GLO_L1_FRE + GLO_L2_FRE);

					// Change unit from meter to cycles
					updSatWL = satUPD.updSatMW/WL_WAVE_GLO;
					updSatLC = satUPD.updSatLC/NL_WAVE_GLO;

					// Get updSatL1/updSatL2, unit:cycles
					updSatL1 = updSatLC - GLO_L2_FRE/(GLO_L1_FRE-GLO_L2_FRE)*updSatWL;
					updSatL2 = updSatLC - GLO_L1_FRE/(GLO_L1_FRE-GLO_L2_FRE)*updSatWL;

					// Convert from cycles to meters
					updSatL1 = updSatL1 * L1_WAVE_GLO;
					updSatL2 = updSatL2 * L2_WAVE_GLO;

					(*stv).second[TypeID::updSatL1] = updSatL1;
					(*stv).second[TypeID::updSatL2] = updSatL2;
*/
				}	


			}  // End of 'for (stv = gData.begin(); stv != gData.end(); ++stv)'

			// Remove satellites with missing data
			gData.removeSatID(satRejectedSet);

			// Return
			return gData;

		}
		catch(Exception& u)
		{

			// Throw an exception if something unexpected happens
			ProcessingException e( getClassName() + ":"
				+ StringUtils::asString( getIndex() ) + ":"
				+ u.what() );

			GPSTK_THROW(e);

		}

	}  // End of method 'CorrectBiasWL::Process()'

}

